#include "colorThresholding.h"

PolarCoordinate convertPixelLoc2Polar(
	double* x, double* y, int* sizeX, int* sizeY){

	// Declare setup variables
	PolarCoordinate coordinate;

	// ERROR HANDLING
	if (x == NULL_CT || y == NULL_CT || sizeX == NULL_CT || sizeY == NULL_CT){

		coordinate.radius_cm = ERROR;
		coordinate.angle_rad = ERROR;
		return coordinate;	

	}
	else if (*x < 0 || *y < 0){
		coordinate.radius_cm = ERROR;
		coordinate.angle_rad = ERROR;

		return coordinate;
	} 

	// Change so that pixels are referenced from the center of the image
	// rather than the corner as in most images
	double x_cm = CONVERTCOL2CM(*x);
	double y_cm = CONVERTROW2CM(*y);

	// Convert from rectangular to polar
	coordinate.radius_cm = sqrt(x_cm * x_cm + y_cm * y_cm );
	coordinate.angle_rad = atan2(x_cm, y_cm);

	return coordinate;

}


unsigned char pixelThreshold(
	ColorThreshold* thresh, 
	uchar_l* pixelRed, uchar_l* pixelGreen, uchar_l* pixelBlue){

	if (thresh == NULL_CT || pixelRed == NULL_CT || pixelGreen == NULL_CT
		|| pixelBlue == NULL_CT){
		return ERROR_UCHAR;
	}
	
		
	// Threshold Red pixel
	uchar_l temp = (thresh->redThresh - thresh->redSDev < *pixelRed);
	temp = temp && (thresh->redThresh + thresh->redSDev > *pixelRed);
	
	// Threshold Green pixel
	temp = temp && (thresh->greenThresh - thresh->greenSDev < *pixelGreen);
	temp = temp && (thresh->greenThresh + thresh->greenSDev > *pixelGreen);
	
	// Threshold blue pixel
	temp = temp && (thresh->blueThresh - thresh->blueSDev < *pixelBlue);
	temp = temp && (thresh->blueThresh + thresh->blueSDev > *pixelBlue);	
	
	
	return temp;
}
